#ifdef WINDOWS

#include "../../../new_common.h"
#include "apps/mqtt.h"
#include "ip_addr.h"
#include "mqtt_opts.h"

#define LWIP_MAX(x, y)   (((x) > (y)) ? (x) : (y))
#define LWIP_MIN(x, y)   (((x) < (y)) ? (x) : (y))
#define LWIP_ARRAYSIZE(x)   (sizeof(x)/sizeof((x)[0]))

#define LWIP_ERROR(a,b,c) if(!(b)) { c; } 

#define LWIP_DEBUGF(a,b);
#define LWIP_ASSERT_CORE_LOCKED();
#define LWIP_ASSERT(a,b);

typedef struct altcp_pcb {
	SOCKET sock;
} altcp_pcb;

/** Main packet buffer struct */
struct pbuf {
	/** next pbuf in singly linked pbuf chain */
	struct pbuf *next;

	/** pointer to the actual data in the buffer */
	void *payload;

	/**
	 * total length of this buffer and all next buffers in chain
	 * belonging to the same packet.
	 *
	 * For non-queue packet chains this is the invariant:
	 * p->tot_len == p->len + (p->next? p->next->tot_len: 0)
	 */
	u16_t tot_len;

	/** length of this buffer */
	u16_t len;

	/** pbuf_type as u8_t instead of enum to save space */
	u8_t /*pbuf_type*/ type;

	/** misc flags */
	u8_t flags;

	/**
	 * the reference count always equals the number of pointers
	 * that refer to this pbuf. This can be pointers from an application,
	 * the stack itself, or pbuf->next pointers from a chain.
	 */
	u16_t ref;
};


extern int g_bDoingUnitTestsNow;
bool MQTT_IsFakingOnlineMQTT() {
	return g_bDoingUnitTestsNow;
}
/**
 * MQTT connect flags, only used in CONNECT message
 */
enum mqtt_connect_flag {
	MQTT_CONNECT_FLAG_USERNAME = 1 << 7,
	MQTT_CONNECT_FLAG_PASSWORD = 1 << 6,
	MQTT_CONNECT_FLAG_WILL_RETAIN = 1 << 5,
	MQTT_CONNECT_FLAG_WILL = 1 << 2,
	MQTT_CONNECT_FLAG_CLEAN_SESSION = 1 << 1
};

/**
 * MQTT client connection states
 */
enum {
	TCP_DISCONNECTED,
	TCP_CONNECTING,
	MQTT_CONNECTING,
	MQTT_CONNECTED
};


/**
 * MQTT control message types
 */
enum mqtt_message_type {
	MQTT_MSG_TYPE_CONNECT = 1,
	MQTT_MSG_TYPE_CONNACK = 2,
	MQTT_MSG_TYPE_PUBLISH = 3,
	MQTT_MSG_TYPE_PUBACK = 4,
	MQTT_MSG_TYPE_PUBREC = 5,
	MQTT_MSG_TYPE_PUBREL = 6,
	MQTT_MSG_TYPE_PUBCOMP = 7,
	MQTT_MSG_TYPE_SUBSCRIBE = 8,
	MQTT_MSG_TYPE_SUBACK = 9,
	MQTT_MSG_TYPE_UNSUBSCRIBE = 10,
	MQTT_MSG_TYPE_UNSUBACK = 11,
	MQTT_MSG_TYPE_PINGREQ = 12,
	MQTT_MSG_TYPE_PINGRESP = 13,
	MQTT_MSG_TYPE_DISCONNECT = 14
};

/**
 * @ingroup pbuf
 * Copy (part of) the contents of a packet buffer
 * to an application supplied buffer.
 *
 * @param buf the pbuf from which to copy data
 * @param dataptr the application supplied buffer
 * @param len length of data to copy (dataptr must be big enough). No more
 * than buf->tot_len will be copied, irrespective of len
 * @param offset offset into the packet buffer from where to begin copying len bytes
 * @return the number of bytes copied, or 0 on failure
 */
u16_t
pbuf_copy_partial(const struct pbuf *buf, void *dataptr, u16_t len, u16_t offset)
{
	const struct pbuf *p;
	u16_t left = 0;
	u16_t buf_copy_len;
	u16_t copied_total = 0;

	LWIP_ERROR("pbuf_copy_partial: invalid buf", (buf != NULL), return 0;);
	LWIP_ERROR("pbuf_copy_partial: invalid dataptr", (dataptr != NULL), return 0;);

	/* Note some systems use byte copy if dataptr or one of the pbuf payload pointers are unaligned. */
	for (p = buf; len != 0 && p != NULL; p = p->next) {
		if ((offset != 0) && (offset >= p->len)) {
			/* don't copy from this buffer -> on to the next */
			offset = (u16_t)(offset - p->len);
		}
		else {
			/* copy from this buffer. maybe only partially. */
			buf_copy_len = (u16_t)(p->len - offset);
			if (buf_copy_len > len) {
				buf_copy_len = len;
			}
			/* copy the necessary parts of the buffer */
			memcpy(&((char *)dataptr)[left], &((char *)p->payload)[offset], buf_copy_len);
			copied_total = (u16_t)(copied_total + buf_copy_len);
			left = (u16_t)(left + buf_copy_len);
			len = (u16_t)(len - buf_copy_len);
			offset = 0;
		}
	}
	return copied_total;
}
/* Actual implementation of pbuf_skip() but returning const pointer... */
static const struct pbuf *
pbuf_skip_const(const struct pbuf *in, u16_t in_offset, u16_t *out_offset)
{
	u16_t offset_left = in_offset;
	const struct pbuf *q = in;

	/* get the correct pbuf */
	while ((q != NULL) && (q->len <= offset_left)) {
		offset_left = (u16_t)(offset_left - q->len);
		q = q->next;
	}
	if (out_offset != NULL) {
		*out_offset = offset_left;
	}
	return q;
}
/*
*@ingroup pbuf
* Get one byte from the specified position in a pbuf
*
* @param p pbuf to parse
* @param offset offset into p of the byte to return
*@return byte at an offset into p[0..0xFF] OR negative if 'offset' >= p->tot_len
*/
int
pbuf_try_get_at(const struct pbuf *p, u16_t offset)
{
	u16_t q_idx;
	const struct pbuf *q = pbuf_skip_const(p, offset, &q_idx);

	/* return requested data if pbuf is OK */
	if ((q != NULL) && (q->len > q_idx)) {
		return ((u8_t *)q->payload)[q_idx];
	}
	return -1;
}
/**
 * @ingroup pbuf
 * Get one byte from the specified position in a pbuf
 * WARNING: returns zero for offset >= p->tot_len
 *
 * @param p pbuf to parse
 * @param offset offset into p of the byte to return
 * @return byte at an offset into p OR ZERO IF 'offset' >= p->tot_len
 */
u8_t
pbuf_get_at(const struct pbuf *p, u16_t offset)
{
	int ret = pbuf_try_get_at(p, offset);
	if (ret >= 0) {
		return (u8_t)ret;
	}
	return 0;
}
/** Helpers to extract control packet type and qos from first byte in fixed header */
#define MQTT_CTL_PACKET_TYPE(fixed_hdr_byte0) ((fixed_hdr_byte0 & 0xf0) >> 4)
#define MQTT_CTL_PACKET_QOS(fixed_hdr_byte0) ((fixed_hdr_byte0 & 0x6) >> 1)
/*--------------------------------------------------------------------------------------------------------------------- */
/* Output message build helpers */

/** Add single item to ring buffer */
static void mqtt_ringbuf_put(struct mqtt_ringbuf_t *rb, u8_t item)
{
	rb->buf[rb->put] = (item);                    /* save byte */
	rb->put++;                                    /* increase pointer */
	if (rb->put >= MQTT_OUTPUT_RINGBUF_SIZE)
	{
		rb->put -= MQTT_OUTPUT_RINGBUF_SIZE;      /* wrap around */
	}
	/* overflow check */
	if (rb->put == rb->get)
	{
		rb->get++;
		if (rb->get >= MQTT_OUTPUT_RINGBUF_SIZE)
		{
			rb->get -= MQTT_OUTPUT_RINGBUF_SIZE;
		}
	}
}

static void
mqtt_output_append_u8(struct mqtt_ringbuf_t *rb, u8_t value)
{
	mqtt_ringbuf_put(rb, value);
}

static
void mqtt_output_append_u16(struct mqtt_ringbuf_t *rb, u16_t value)
{
	mqtt_ringbuf_put(rb, (u8_t)((value >> 8) & 0xff));
	mqtt_ringbuf_put(rb, (u8_t)(value & 0xff));
}

static void
mqtt_output_append_buf(struct mqtt_ringbuf_t *rb, const void *data, u16_t length)
{
	u16_t n;
	for (n = 0; n < length; n++)
	{
		mqtt_ringbuf_put(rb, ((const u8_t *)data)[n]);
	}
}

static void
mqtt_output_append_string(struct mqtt_ringbuf_t *rb, const char *str, u16_t length)
{
	u16_t n;
	mqtt_ringbuf_put(rb, (u8_t)((length >> 8) & 0xFF));
	mqtt_ringbuf_put(rb, (u8_t)(length & 0xff));
	for (n = 0; n < length; n++)
	{
		mqtt_ringbuf_put(rb, ((u8_t *)str)[n]);
	}
}


/** Return number of bytes free in ring buffer */
int mqtt_ringbuf_free(struct mqtt_ringbuf_t *rb)
{
	if (rb->put > rb->get)
	{
		/* unwrapped pointers, free (out->end) + (start->get) */
		return ((MQTT_OUTPUT_RINGBUF_SIZE - rb->put) + rb->get);
	}
	else if (rb->put < rb->get)
	{
		/* wrapped pointers, free space between pointers */
		return (rb->get - rb->put);
	}
	return MQTT_OUTPUT_RINGBUF_SIZE; /* buffer empty */
}

/**
 * Delete request item
 * @param r Request item to delete
 */
static void
mqtt_delete_request(struct mqtt_request_t *r)
{
	if (r != NULL) {
		r->next = r;
	}
}
/**
 * Append fixed header
 * @param rb Output ring buffer
 * @param msg_type see enum mqtt_message_type
 * @param fdup MQTT DUP flag
 * @param fqos MQTT QoS field
 * @param fretain MQTT retain flag
 * @param r_length Remaining length after fixed header
 */

static void
mqtt_output_append_fixed_header(struct mqtt_ringbuf_t *rb, u8_t msg_type, u8_t fdup,
	u8_t fqos, u8_t fretain, u16_t r_length)
{
	/* Start with control byte */
	mqtt_output_append_u8(rb, (((msg_type & 0x0f) << 4) | ((fdup & 1) << 3) | ((fqos & 3) << 1) | (fretain & 1)));
	/* Encode remaining length field */
	do {
		mqtt_output_append_u8(rb, (r_length & 0x7f) | (r_length >= 128 ? 0x80 : 0));
		r_length >>= 7;
	} while (r_length > 0);
}


/**
 * Check output buffer space
 * @param rb Output ring buffer
 * @param r_length Remaining length after fixed header
 * @return 1 if message will fit, 0 if not enough buffer space
 */
static u8_t
mqtt_output_check_space(struct mqtt_ringbuf_t *rb, u16_t r_length)
{
	/* Start with length of type byte + remaining length */
	u16_t total_len = 1 + r_length;

	LWIP_ASSERT("mqtt_output_check_space: rb != NULL", rb != NULL);

	/* Calculate number of required bytes to contain the remaining bytes field and add to total*/
	do {
		total_len++;
		r_length >>= 7;
	} while (r_length > 0);

	return (total_len <= mqtt_ringbuf_free(rb));
}
/**
 * Free all request items
 * @param tail Pointer to request queue tail pointer
 */
static void
mqtt_clear_requests(struct mqtt_request_t **tail)
{
	struct mqtt_request_t *iter, *next;
	LWIP_ASSERT("mqtt_clear_requests: tail != NULL", tail != NULL);
	for (iter = *tail; iter != NULL; iter = next) {
		next = iter->next;
		mqtt_delete_request(iter);
	}
	*tail = NULL;
}
/**
 * Initialize all request items
 * @param r_objs Pointer to request objects
 * @param r_objs_len Number of array entries
 */
static void
mqtt_init_requests(struct mqtt_request_t *r_objs, size_t r_objs_len)
{
	u8_t n;
	LWIP_ASSERT("mqtt_init_requests: r_objs != NULL", r_objs != NULL);
	for (n = 0; n < r_objs_len; n++) {
		/* Item pointing to itself indicates unused */
		r_objs[n].next = &r_objs[n];
	}
}
/** Connect to server */
err_t mqtt_client_connect(mqtt_client_t *client, const ip_addr_t *ip_addr, u16_t port, mqtt_connection_cb_t cb, void *arg,
                   const struct mqtt_connect_client_info_t *client_info) {

	err_t err;
	size_t len;
	u16_t client_id_length;
	/* Length is the sum of 2+"MQTT", protocol level, flags and keep alive */
	u16_t remaining_length = 2 + 4 + 1 + 1 + 2;
	u8_t flags = 0, will_topic_len = 0, will_msg_len = 0;
	u16_t client_user_len = 0, client_pass_len = 0;


	if (MQTT_IsFakingOnlineMQTT())
		return 0;

	LWIP_ASSERT_CORE_LOCKED();
	LWIP_ASSERT("mqtt_client_connect: client != NULL", client != NULL);
	LWIP_ASSERT("mqtt_client_connect: ip_addr != NULL", ip_addr != NULL);
	LWIP_ASSERT("mqtt_client_connect: client_info != NULL", client_info != NULL);
	LWIP_ASSERT("mqtt_client_connect: client_info->client_id != NULL", client_info->client_id != NULL);

	if (client->conn_state != TCP_DISCONNECTED) {
		LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_client_connect: Already connected\n"));
		return ERR_ISCONN;
	}

	/* Wipe clean */
	memset(client, 0, sizeof(mqtt_client_t));
	client->connect_arg = arg;
	client->connect_cb = cb;
	client->keep_alive = client_info->keep_alive;
	mqtt_init_requests(client->req_list, LWIP_ARRAYSIZE(client->req_list));

	/* Build connect message */
	if (client_info->will_topic != NULL && client_info->will_msg != NULL) {
		flags |= MQTT_CONNECT_FLAG_WILL;
		flags |= (client_info->will_qos & 3) << 3;
		if (client_info->will_retain) {
			flags |= MQTT_CONNECT_FLAG_WILL_RETAIN;
		}
		len = strlen(client_info->will_topic);
		LWIP_ERROR("mqtt_client_connect: client_info->will_topic length overflow", len <= 0xFF, return ERR_VAL);
		LWIP_ERROR("mqtt_client_connect: client_info->will_topic length must be > 0", len > 0, return ERR_VAL);
		will_topic_len = (u8_t)len;
		len = strlen(client_info->will_msg);
		LWIP_ERROR("mqtt_client_connect: client_info->will_msg length overflow", len <= 0xFF, return ERR_VAL);
		will_msg_len = (u8_t)len;
		len = remaining_length + 2 + will_topic_len + 2 + will_msg_len;
		LWIP_ERROR("mqtt_client_connect: remaining_length overflow", len <= 0xFFFF, return ERR_VAL);
		remaining_length = (u16_t)len;
	}
	if (client_info->client_user != NULL) {
		flags |= MQTT_CONNECT_FLAG_USERNAME;
		len = strlen(client_info->client_user);
		LWIP_ERROR("mqtt_client_connect: client_info->client_user length overflow", len <= 0xFFFF, return ERR_VAL);
		LWIP_ERROR("mqtt_client_connect: client_info->client_user length must be > 0", len > 0, return ERR_VAL);
		client_user_len = (u16_t)len;
		len = remaining_length + 2 + client_user_len;
		LWIP_ERROR("mqtt_client_connect: remaining_length overflow", len <= 0xFFFF, return ERR_VAL);
		remaining_length = (u16_t)len;
	}
	if (client_info->client_pass != NULL) {
		flags |= MQTT_CONNECT_FLAG_PASSWORD;
		len = strlen(client_info->client_pass);
		LWIP_ERROR("mqtt_client_connect: client_info->client_pass length overflow", len <= 0xFFFF, return ERR_VAL);
		LWIP_ERROR("mqtt_client_connect: client_info->client_pass length must be > 0", len > 0, return ERR_VAL);
		client_pass_len = (u16_t)len;
		len = remaining_length + 2 + client_pass_len;
		LWIP_ERROR("mqtt_client_connect: remaining_length overflow", len <= 0xFFFF, return ERR_VAL);
		remaining_length = (u16_t)len;
	}

	/* Don't complicate things, always connect using clean session */
	flags |= MQTT_CONNECT_FLAG_CLEAN_SESSION;

	len = strlen(client_info->client_id);
	LWIP_ERROR("mqtt_client_connect: client_info->client_id length overflow", len <= 0xFFFF, return ERR_VAL);
	client_id_length = (u16_t)len;
	len = remaining_length + 2 + client_id_length;
	LWIP_ERROR("mqtt_client_connect: remaining_length overflow", len <= 0xFFFF, return ERR_VAL);
	remaining_length = (u16_t)len;

	if (mqtt_output_check_space(&client->output, remaining_length) == 0) {
		//if (client->output.g_mutex != NULL)
		//	vSemaphoreDelete(client->output.g_mutex);
		//client->output.g_mutex = NULL;
		return ERR_MEM;
	}

	SOCKET s;
	struct sockaddr_in server;


	//Create a socket
	if ((s = socket(AF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET)
	{
		printf("Could not create socket : %d", WSAGetLastError());
	}

	printf("Socket created.\n");

	//server.sin_addr.s_addr = inet_addr("192.168.0.113");
	memcpy(&server.sin_addr, ip_addr, 4);
	server.sin_family = AF_INET;
	server.sin_port = htons(port);

	// Set the socket to non-blocking mode
	unsigned long nonBlocking = 1;
	if (ioctlsocket(s, FIONBIO, &nonBlocking) != 0)
	{
		printf("ioctlsocket failed with error: %d\n", WSAGetLastError());
		return 1;
	}
	int aliveToggle = 1;
	int res = setsockopt(s, SOL_SOCKET, SO_KEEPALIVE, (char*)&aliveToggle, sizeof(aliveToggle));
	if (res != 0) {
		printf("Sock opt error\n");
	}

	//Connect to remote server
	if (connect(s, (struct sockaddr *)&server, sizeof(server)) < 0)
	{
		//puts("connect error");
		//return 1;
	}
	client->conn = (altcp_pcb*)malloc(sizeof(altcp_pcb));
	client->conn->sock = s;

	client->conn_state = TCP_CONNECTING;

	//if (xSemaphoreTake(client->output.g_mutex, portMAX_DELAY) == pdTRUE)
	{
		/* Append fixed header */
		mqtt_output_append_fixed_header(&client->output, MQTT_MSG_TYPE_CONNECT, 0, 0, 0, remaining_length);
		/* Append Protocol string */
		mqtt_output_append_string(&client->output, "MQTT", 4);
		/* Append Protocol level */
		mqtt_output_append_u8(&client->output, 4);
		/* Append connect flags */
		mqtt_output_append_u8(&client->output, flags);
		/* Append keep-alive */
		mqtt_output_append_u16(&client->output, client_info->keep_alive);
		/* Append client id */
		mqtt_output_append_string(&client->output, client_info->client_id, client_id_length);
		/* Append will message if used */
		if ((flags & MQTT_CONNECT_FLAG_WILL) != 0) {
			mqtt_output_append_string(&client->output, client_info->will_topic, will_topic_len);
			mqtt_output_append_string(&client->output, client_info->will_msg, will_msg_len);
		}
		/* Append user name if given */
		if ((flags & MQTT_CONNECT_FLAG_USERNAME) != 0) {
			mqtt_output_append_string(&client->output, client_info->client_user, client_user_len);
		}
		/* Append password if given */
		if ((flags & MQTT_CONNECT_FLAG_PASSWORD) != 0) {
			mqtt_output_append_string(&client->output, client_info->client_pass, client_pass_len);
		}
//		xSemaphoreGive(client->output.g_mutex);
	}
	return ERR_OK;
}
/*--------------------------------------------------------------------------------------------------------------------- */
/* Output ring buffer */


/** Return pointer to ring buffer get position */
static u8_t *mqtt_ringbuf_get_ptr(struct mqtt_ringbuf_t *rb)
{
	return &rb->buf[rb->get];
}

/** Return number of bytes in ring buffer */
static u16_t mqtt_ringbuf_len(struct mqtt_ringbuf_t *rb)
{
	if (rb->put > rb->get)
	{
		/* unwrapped pointers * */
		return (int)(rb->put - rb->get);
	}
	else if (rb->put < rb->get) {
		/* wrapped pointers */
		return (int)((MQTT_OUTPUT_RINGBUF_SIZE - rb->get) + rb->put);
	}
	return 0; /* buffer empty */
}

static void mqtt_ringbuf_advance_get_idx(struct mqtt_ringbuf_t *rb, u16_t len)
{
	LWIP_ASSERT("mqtt_ringbuf_advance_get_idx: len < MQTT_OUTPUT_RINGBUF_SIZE", len < MQTT_OUTPUT_RINGBUF_SIZE);

	int _len = mqtt_ringbuf_len(rb);
	if (len > _len)
	{
		len = _len;
	}
	/* move pointer */
	rb->get += len;
	/* wrapp arround */
	if (rb->get >= MQTT_OUTPUT_RINGBUF_SIZE)
	{
		rb->get -= MQTT_OUTPUT_RINGBUF_SIZE;
	}
}

/** Return number of bytes possible to read without wrapping around */
int mqtt_ringbuf_linear_read_length(struct mqtt_ringbuf_t *rb)
{
	if (rb->put > rb->get)
	{
		/* unwrapped pointers * */
		return (int)(rb->put - rb->get);
	}
	else if (rb->put < rb->get)
	{
		/* wrapped pointers */
		return (int)(MQTT_OUTPUT_RINGBUF_SIZE - rb->get);
	}
	return 0;
}
int altcp_sndbuf(void *v) {
	return 99999;
}
int altcp_output(void *v) {
	return 0;
}
/**
 * Try send as many bytes as possible from output ring buffer
 * @param rb Output ring buffer
 * @param tpcb TCP connection handle
 */
static void
mqtt_output_send(struct mqtt_ringbuf_t *rb, struct altcp_pcb *tpcb)
{
	err_t err;
	u8_t wrap = 0;
	u16_t ringbuf_lin_len = 0;
	u16_t ringbuf_len = 0;
	u16_t send_len = 0;
	int myTestLen;

	LWIP_ASSERT("mqtt_output_send: tpcb != NULL", tpcb != NULL);

	{
		ringbuf_lin_len = mqtt_ringbuf_linear_read_length(rb);
		ringbuf_len = mqtt_ringbuf_len(rb);
		send_len = altcp_sndbuf(tpcb);

		if (send_len == 0 || ringbuf_lin_len == 0)
		{
			
			return;
		}

		LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_output_send: tcp_sndbuf: %d bytes, ringbuf_linear_available: %d, get %d, put %d len %d\n",
			send_len, ringbuf_lin_len, rb->get, rb->put, ringbuf_len));

		/*    {
			  char tmp[128];
			  sprintf(tmp,"mqtt_output_send: tcp_sndbuf: %d bytes, ringbuf_linear_available: %d, get %d, put %d len %d\n\r",
					  send_len, ringbuf_lin_len, ((rb)->get), ((rb)->put), ringbuf_len);
			  LWIP_DEBUGF(MQTT_DEBUG_CUSTOM, (tmp) );
			}*/

		if (send_len > ringbuf_lin_len)
		{
			/* Space in TCP output buffer is larger than available in ring buffer linear portion */
			send_len = ringbuf_lin_len;
			/* Wrap around if more data in ring buffer after linear portion */
			wrap = (mqtt_ringbuf_len(rb) > ringbuf_lin_len);
		}

		//err = 0;// altcp_write(tpcb, mqtt_ringbuf_get_ptr(rb), send_len, TCP_WRITE_FLAG_COPY | ((wrap != 0) ? TCP_WRITE_FLAG_MORE : 0));

		err = ERR_OK;
		myTestLen = send(tpcb->sock, mqtt_ringbuf_get_ptr(rb), send_len, 0);

		if (err == ERR_OK)
		{
			mqtt_ringbuf_advance_get_idx(rb, send_len);
			if (wrap != 0)
			{
				/* Use the lesser one of ring buffer linear length and TCP send buffer size */
				send_len = LWIP_MIN(altcp_sndbuf(tpcb), mqtt_ringbuf_linear_read_length(rb));
				//err = altcp_write(tpcb, mqtt_ringbuf_get_ptr(rb), send_len, TCP_WRITE_FLAG_COPY);
				err = ERR_OK;
				myTestLen = send(tpcb->sock, mqtt_ringbuf_get_ptr(rb), send_len, 0);
				if (err == ERR_OK)
				{
					mqtt_ringbuf_advance_get_idx(rb, send_len);
				}
			}
			if (err == ERR_OK)
			{
				/* Flush */
				err = altcp_output(tpcb);
			}
		}

		if (err != ERR_OK)
		{
			LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_output_send: Send failed with err %d (\"%s\")\n", err, "lwip_strerr(err)"));
			{
				char tmp[128];
				sprintf(tmp, "mqtt_output_send: Send failed with err %d (\"%s\")\n\r", err,"lwip_strerr(err)");
				LWIP_DEBUGF(MQTT_DEBUG_CUSTOM, (tmp));
			}
		}
		//xSemaphoreGive(rb->g_mutex);
	}
}
static void mqtt_close(mqtt_client_t *client, mqtt_connection_status_t reason)
{
	LWIP_ASSERT("mqtt_close: client != NULL", client != NULL);

	/* Bring down TCP connection if not already done */
	if (client->conn != NULL)
	{
		//err_t res;
		//altcp_recv(client->conn, NULL);
		//altcp_err(client->conn, NULL);
		//altcp_sent(client->conn, NULL);
		//res = altcp_close(client->conn);
		//if (res != ERR_OK) {
		//	altcp_abort(client->conn);
		//	LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_close: Close err=%s\n", lwip_strerr(res)));
		//}
		closesocket(client->conn->sock);
		client->conn->sock = 0;
		free(client->conn);
		client->conn = NULL;
	}
	/* Remove all pending requests */
	mqtt_clear_requests(&client->pend_req_queue);
	/* Stop cyclic timer */
	//sys_untimeout(mqtt_cyclic_timer, client);

	/* Notify upper layer of disconnection if changed state */
	if (client->conn_state != TCP_DISCONNECTED)
	{
		client->conn_state = TCP_DISCONNECTED;
		if (client->connect_cb != NULL)
		{
			client->connect_cb(client, client->connect_arg, reason);
		}
	}
	//if (client->output.g_mutex != NULL)
	//	vSemaphoreDelete(client->output.g_mutex);
	//client->output.g_mutex = NULL;
}
/** Disconnect from server */
void mqtt_disconnect(mqtt_client_t *client) {
	if (MQTT_IsFakingOnlineMQTT())
		return;
	/* If connection in not already closed */
	if (client->conn_state != TCP_DISCONNECTED) {
		/* Set conn_state before calling mqtt_close to prevent callback from being called */
		client->conn_state = TCP_DISCONNECTED;
		mqtt_close(client, (mqtt_connection_status_t)0);
	}
}

mqtt_client_t *g_clients[128] = { 0 };
int g_numClients = 0;
/** Create new client */
mqtt_client_t *mqtt_client_new(void) {
	mqtt_client_t *r;
	r = (mqtt_client_t*)malloc(sizeof(mqtt_client_t));
	g_clients[g_numClients] = r;
	g_numClients++;
	memset(r, 0, sizeof(mqtt_client_t));
	return r;
}

/** Check connection status */
u8_t mqtt_client_is_connected(mqtt_client_t *client) {
	if (MQTT_IsFakingOnlineMQTT())
		return 1;
	return client->conn_state == MQTT_CONNECTED;
}

/** Set callback to call for incoming publish */
void mqtt_set_inpub_callback(mqtt_client_t *client, mqtt_incoming_publish_cb_t pub_cb,
                             mqtt_incoming_data_cb_t data_cb, void *arg) {
	if (MQTT_IsFakingOnlineMQTT())
		return ;
	client->data_cb = data_cb;
	client->pub_cb = pub_cb;
	client->inpub_arg = arg;
	return ;
}


/**
 * Generate MQTT packet identifier
 * @param client MQTT client
 * @return New packet identifier, range 1 to 65535
 */
static u16_t
msg_generate_packet_id(mqtt_client_t *client)
{
	client->pkt_id_seq++;
	if (client->pkt_id_seq == 0) {
		client->pkt_id_seq++;
	}
	return client->pkt_id_seq;
}

/**
 * Append request to pending request queue
 * @param tail Pointer to request queue tail pointer
 * @param r Request to append
 */
static void
mqtt_append_request(struct mqtt_request_t **tail, struct mqtt_request_t *r)
{
	struct mqtt_request_t *head = NULL;
	short time_before = 0;
	struct mqtt_request_t *iter;

	LWIP_ASSERT("mqtt_append_request: tail != NULL", tail != NULL);

	/* Iterate trough queue to find head, and count total timeout time */
	for (iter = *tail; iter != NULL; iter = iter->next) {
		time_before += iter->timeout_diff;
		head = iter;
	}

	LWIP_ASSERT("mqtt_append_request: time_before <= MQTT_REQ_TIMEOUT", time_before <= MQTT_REQ_TIMEOUT);
	r->timeout_diff = MQTT_REQ_TIMEOUT - time_before;
	if (head == NULL) {
		*tail = r;
	}
	else {
		head->next = r;
	}
}


/**
 * Remove a request item with a specific packet identifier from request queue
 * @param tail Pointer to request queue tail pointer
 * @param pkt_id Packet identifier of request to take
 * @return Request item if found, NULL if not
 */
static struct mqtt_request_t *
mqtt_take_request(struct mqtt_request_t **tail, u16_t pkt_id)
{
	struct mqtt_request_t *iter = NULL, *prev = NULL;
	LWIP_ASSERT("mqtt_take_request: tail != NULL", tail != NULL);
	/* Search all request for pkt_id */
	for (iter = *tail; iter != NULL; iter = iter->next) {
		if (iter->pkt_id == pkt_id) {
			break;
		}
		prev = iter;
	}

	/* If request was found */
	if (iter != NULL) {
		/* unchain */
		if (prev == NULL) {
			*tail = iter->next;
		}
		else {
			prev->next = iter->next;
		}
		/* If exists, add remaining timeout time for the request to next */
		if (iter->next != NULL) {
			iter->next->timeout_diff += iter->timeout_diff;
		}
		iter->next = NULL;
	}
	return iter;
}
/*--------------------------------------------------------------------------------------------------------------------- */
/* Request queue */

/**
 * Create request item
 * @param r_objs Pointer to request objects
 * @param r_objs_len Number of array entries
 * @param pkt_id Packet identifier of request
 * @param cb Packet callback to call when requests lifetime ends
 * @param arg Parameter following callback
 * @return Request or NULL if failed to create
 */
static struct mqtt_request_t *
mqtt_create_request(struct mqtt_request_t *r_objs, size_t r_objs_len, u16_t pkt_id, mqtt_request_cb_t cb, void *arg)
{
	struct mqtt_request_t *r = NULL;
	u8_t n;
	LWIP_ASSERT("mqtt_create_request: r_objs != NULL", r_objs != NULL);
	for (n = 0; n < r_objs_len; n++) {
		/* Item point to itself if not in use */
		if (r_objs[n].next == &r_objs[n]) {
			r = &r_objs[n];
			r->next = NULL;
			r->cb = cb;
			r->arg = arg;
			r->pkt_id = pkt_id;
			break;
		}
	}
	return r;
}
/** Common function for subscribe and unsubscribe */
err_t mqtt_sub_unsub(mqtt_client_t *client, const char *topic, u8_t qos, mqtt_request_cb_t cb, void *arg, u8_t sub) {

	if (MQTT_IsFakingOnlineMQTT())
		return;
	size_t topic_strlen;
	size_t total_len;
	u16_t topic_len;
	u16_t remaining_length;
	u16_t pkt_id;
	struct mqtt_request_t *r;

	LWIP_ASSERT_CORE_LOCKED();
	LWIP_ASSERT("mqtt_sub_unsub: client != NULL", client);
	LWIP_ASSERT("mqtt_sub_unsub: topic != NULL", topic);

	topic_strlen = strlen(topic);
	LWIP_ERROR("mqtt_sub_unsub: topic length overflow", (topic_strlen <= (0xFFFF - 2)), return ERR_ARG);
	topic_len = (u16_t)topic_strlen;
	/* Topic string, pkt_id, qos for subscribe */
	total_len = topic_len + 2 + 2 + (sub != 0);
	LWIP_ERROR("mqtt_sub_unsub: total length overflow", (total_len <= 0xFFFF), return ERR_ARG);
	remaining_length = (u16_t)total_len;

	LWIP_ASSERT("mqtt_sub_unsub: qos < 3", qos < 3);
	if (client->conn_state == TCP_DISCONNECTED) {
		LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_sub_unsub: Can not (un)subscribe in disconnected state\n"));
		return ERR_CONN;
	}

	pkt_id = msg_generate_packet_id(client);
	r = mqtt_create_request(client->req_list, LWIP_ARRAYSIZE(client->req_list), pkt_id, cb, arg);
	if (r == NULL) {
		return ERR_MEM;
	}

	if (mqtt_output_check_space(&client->output, remaining_length) == 0) {
		mqtt_delete_request(r);
		return ERR_MEM;
	}

	LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_sub_unsub: Client (un)subscribe to topic \"%s\", id: %d\n", topic, pkt_id));

	mqtt_output_append_fixed_header(&client->output, sub ? MQTT_MSG_TYPE_SUBSCRIBE : MQTT_MSG_TYPE_UNSUBSCRIBE, 0, 1, 0, remaining_length);
	/* Packet id */
	mqtt_output_append_u16(&client->output, pkt_id);
	/* Topic */
	mqtt_output_append_string(&client->output, topic, topic_len);
	/* QoS */
	if (sub != 0) {
		mqtt_output_append_u8(&client->output, LWIP_MIN(qos, 2));
	}

	mqtt_append_request(&client->pend_req_queue, r);
	mqtt_output_send(&client->output, client->conn);
	return ERR_OK;
}

void SIM_OnMQTTPublish(const char *topic, const char *value, int len, int qos, bool bRetain);

/** Publish data to topic */
err_t mqtt_publish(mqtt_client_t *client, const char *topic, const void *payload, u16_t payload_length, u8_t qos, u8_t retain,
				   mqtt_request_cb_t cb, void *arg) {
#if 1
	{
		FILE *f = fopen("lastMQTTPublishSentByOBK.txt", "wb");
		if (f) {
			fwrite(payload, 1, payload_length, f);
			fclose(f);
		}
	}
#endif
	if (MQTT_IsFakingOnlineMQTT()) {
		// on Windows simulator, forward MQTT publish for unit testing
		SIM_OnMQTTPublish(topic, payload, payload_length, qos, retain);
		return 0;
	}

	struct mqtt_request_t *r;
	u16_t pkt_id;
	size_t topic_strlen;
	size_t total_len;
	u16_t topic_len;
	u16_t remaining_length;

	LWIP_ASSERT_CORE_LOCKED();
	LWIP_ASSERT("mqtt_publish: client != NULL", client);
	LWIP_ASSERT("mqtt_publish: topic != NULL", topic);
	LWIP_ERROR("mqtt_publish: TCP disconnected", (client->conn_state != TCP_DISCONNECTED), return ERR_CONN);

	topic_strlen = strlen(topic);
	LWIP_ERROR("mqtt_publish: topic length overflow", (topic_strlen <= (0xFFFF - 2)), return ERR_ARG);
	topic_len = (u16_t)topic_strlen;
	total_len = 2 + topic_len + payload_length;

	if (qos > 0) {
		total_len += 2;
		/* Generate pkt_id id for QoS1 and 2 */
		pkt_id = msg_generate_packet_id(client);
	}
	else {
		/* Use reserved value pkt_id 0 for QoS 0 in request handle */
		pkt_id = 0;
	}
	LWIP_ERROR("mqtt_publish: total length overflow", (total_len <= 0xFFFF), return ERR_ARG);
	remaining_length = (u16_t)total_len;

	LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_publish: Publish with payload length %d to topic \"%s\"\n", payload_length, topic));

	r = mqtt_create_request(client->req_list, LWIP_ARRAYSIZE(client->req_list), pkt_id, cb, arg);
	if (r == NULL)
	{
		LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_publish: request queue full\n"));
		return ERR_MEM;
	}

	if (mqtt_output_check_space(&client->output, remaining_length) == 0)
	{
		LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_publish: Ring buffer full\n"));
		mqtt_delete_request(r);
		return ERR_MEM;
	}

	//if (xSemaphoreTake(client->output.g_mutex, portMAX_DELAY) == pdTRUE)
	{
		/* Append fixed header */
		mqtt_output_append_fixed_header(&client->output, MQTT_MSG_TYPE_PUBLISH, 0, qos, retain, remaining_length);

		/* Append Topic */
		mqtt_output_append_string(&client->output, topic, topic_len);

		/* Append packet if for QoS 1 and 2*/
		if (qos > 0)
		{
			mqtt_output_append_u16(&client->output, pkt_id);
		}

		/* Append optional publish payload */
		if ((payload != NULL) && (payload_length > 0))
		{
			mqtt_output_append_buf(&client->output, payload, payload_length);
		}
		//xSemaphoreGive(client->output.g_mutex);
	}

	mqtt_append_request(&client->pend_req_queue, r);
	mqtt_output_send(&client->output, client->conn);
	return ERR_OK;
}


/**
 * Send PUBACK, PUBREC or PUBREL response message
 * @param client MQTT client
 * @param msg PUBACK, PUBREC or PUBREL
 * @param pkt_id Packet identifier
 * @param qos QoS value
 * @return ERR_OK if successful, ERR_MEM if out of memory
 */
static err_t
pub_ack_rec_rel_response(mqtt_client_t *client, u8_t msg, u16_t pkt_id, u8_t qos)
{
	err_t err = ERR_OK;
	if (mqtt_output_check_space(&client->output, 2))
	{
		/*    {
			  char tmp[128];
			  sprintf(tmp,"mqtt_pub_ack_rel_response: msg: %d pkt_id: %d qos: %d\n\r", msg, pkt_id, qos);
			  LWIP_DEBUGF(MQTT_DEBUG_CUSTOM, (tmp) );
			}*/
		//if (xSemaphoreTake(client->output.g_mutex, portMAX_DELAY) == pdTRUE)
		{
			mqtt_output_append_fixed_header(&client->output, msg, 0, qos, 0, 2);
			mqtt_output_append_u16(&client->output, pkt_id);
			//xSemaphoreGive(client->output.g_mutex);
		}
		mqtt_output_send(&client->output, client->conn);
	}
	else {
		LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("pub_ack_rec_rel_response: OOM creating response: %s with pkt_id: %d\n",
			mqtt_msg_type_to_str(msg), pkt_id));
		err = ERR_MEM;

	}
	return err;
}

/**
 * Subscribe response from server
 * @param r Matching request
 * @param result Result code from server
 */
static void
mqtt_incomming_suback(struct mqtt_request_t *r, u8_t result)
{
	if (r->cb != NULL) {
		r->cb(r->arg, result < 3 ? ERR_OK : ERR_ABRT);
	}
}
/**
 * Complete MQTT message received or buffer full
 * @param client MQTT client
 * @param fixed_hdr_len length of fixed header
 * @param length length received part
 * @param remaining_length Remaining length of complete message
 */
static mqtt_connection_status_t
mqtt_message_received(mqtt_client_t *client, u8_t fixed_hdr_len, u16_t length, u32_t remaining_length)
{
	mqtt_connection_status_t res = MQTT_CONNECT_ACCEPTED;

	u8_t *var_hdr_payload = client->rx_buffer + fixed_hdr_len;
	size_t var_hdr_payload_bufsize = sizeof(client->rx_buffer) - fixed_hdr_len;

	/* Control packet type */
	u8_t pkt_type = MQTT_CTL_PACKET_TYPE(client->rx_buffer[0]);
	u16_t pkt_id = 0;

	LWIP_ASSERT("fixed_hdr_len <= client->msg_idx", fixed_hdr_len <= client->msg_idx);
	LWIP_ERROR("buffer length mismatch", fixed_hdr_len + length <= MQTT_VAR_HEADER_BUFFER_LEN,
		return MQTT_CONNECT_DISCONNECTED);

	if (pkt_type == MQTT_MSG_TYPE_CONNACK)
	{
		if (client->conn_state == MQTT_CONNECTING)
		{
			if (length < 2)
			{
				LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received short CONNACK message\n"));
				goto out_disconnect;
			}
			/* Get result code from CONNACK */
			res = (mqtt_connection_status_t)var_hdr_payload[1];
			LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: Connect response code %d\n", res));
			if (res == MQTT_CONNECT_ACCEPTED)
			{
				/* Reset cyclic_tick when changing to connected state */
				client->cyclic_tick = 0;
				client->conn_state = MQTT_CONNECTED;
				/* Notify upper layer */
				if (client->connect_cb != 0)
				{
					client->connect_cb(client, client->connect_arg, res);
				}
			}
		}
		else {
			LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received CONNACK in connected state\n"));
		}
	}
	else if (pkt_type == MQTT_MSG_TYPE_PINGRESP) {
		LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: Received PINGRESP from server\n"));
	}
	else if (pkt_type == MQTT_MSG_TYPE_PUBLISH) {
		u16_t payload_offset = 0;
		u16_t payload_length = length;
		u8_t qos = MQTT_CTL_PACKET_QOS(client->rx_buffer[0]);

		if (client->msg_idx <= MQTT_VAR_HEADER_BUFFER_LEN)
		{
			/* Should have topic and pkt id*/
			u8_t *topic;
			u16_t after_topic;
			u8_t bkp;
			u16_t topic_len;
			u16_t qos_len = (qos ? 2U : 0U);
			if (length < 2 + qos_len)
			{
				LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received short PUBLISH packet\n"));
				goto out_disconnect;
			}
			topic_len = var_hdr_payload[0];
			topic_len = (topic_len << 8) + (u16_t)(var_hdr_payload[1]);
			if ((topic_len > length - (2 + qos_len)) ||
				(topic_len > var_hdr_payload_bufsize - (2 + qos_len)))
			{
				LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received short PUBLISH packet (topic)\n"));
				goto out_disconnect;
			}

			topic = var_hdr_payload + 2;
			printf("WIN MQTT: Topic %s\n", topic);

			after_topic = 2 + topic_len;
			/* Check buffer length, add one byte even for QoS 0 so that zero termination will fit */
			if ((after_topic + (qos ? 2U : 1U)) > var_hdr_payload_bufsize) {
				LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Receive buffer can not fit topic + pkt_id\n"));
				goto out_disconnect;
			}

			/* id for QoS 1 and 2 */
			if (qos > 0) {
				if (length < after_topic + 2U) {
					LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received short PUBLISH packet (after_topic)\n"));
					goto out_disconnect;
				}
				client->inpub_pkt_id = ((u16_t)var_hdr_payload[after_topic] << 8) + (u16_t)var_hdr_payload[after_topic + 1];
				after_topic += 2;
			}
			else {
				client->inpub_pkt_id = 0;
			}
			/* Take backup of byte after topic */
			bkp = topic[topic_len];
			/* Zero terminate string */
			topic[topic_len] = 0;
			/* Payload data remaining in receive buffer */
			payload_length = length - after_topic;
			payload_offset = after_topic;

			LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_incomming_publish: Received message with QoS %d at topic: %s, payload length %"U32_F"\n",
				qos, topic, remaining_length + payload_length));
			if (client->pub_cb != NULL) {
				client->pub_cb(client->inpub_arg, (const char *)topic, remaining_length + payload_length);
			}
			/* Restore byte after topic */
			topic[topic_len] = bkp;
		}
		if (payload_length > 0 || remaining_length == 0) {
			if (length < (size_t)(payload_offset + payload_length)) {
				LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received short packet (payload)\n"));
				goto out_disconnect;
			}
			if (client->data_cb != NULL) {
				client->data_cb(client->inpub_arg, var_hdr_payload + payload_offset, payload_length, remaining_length == 0 ? MQTT_DATA_FLAG_LAST : 0);
			}
			/* Reply if QoS > 0 */
			if (remaining_length == 0 && qos > 0) {
				/* Send PUBACK for QoS 1 or PUBREC for QoS 2 */
				u8_t resp_msg = (qos == 1) ? MQTT_MSG_TYPE_PUBACK : MQTT_MSG_TYPE_PUBREC;
				LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_incomming_publish: Sending publish response: %s with pkt_id: %d\n",
					mqtt_msg_type_to_str(resp_msg), client->inpub_pkt_id));
				pub_ack_rec_rel_response(client, resp_msg, client->inpub_pkt_id, 0);
			}
		}
	}
	else {
		/* Get packet identifier */
		pkt_id = (u16_t)var_hdr_payload[0] << 8;
		pkt_id |= (u16_t)var_hdr_payload[1];
		if (pkt_id == 0) {
			LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Got message with illegal packet identifier: 0\n"));
			goto out_disconnect;
		}
		if (pkt_type == MQTT_MSG_TYPE_PUBREC) {
			LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: PUBREC, sending PUBREL with pkt_id: %d\n", pkt_id));
			pub_ack_rec_rel_response(client, MQTT_MSG_TYPE_PUBREL, pkt_id, 1);

		}
		else if (pkt_type == MQTT_MSG_TYPE_PUBREL) {
			LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: PUBREL, sending PUBCOMP response with pkt_id: %d\n", pkt_id));
			pub_ack_rec_rel_response(client, MQTT_MSG_TYPE_PUBCOMP, pkt_id, 0);

		}
		else if (pkt_type == MQTT_MSG_TYPE_SUBACK || pkt_type == MQTT_MSG_TYPE_UNSUBACK ||
			pkt_type == MQTT_MSG_TYPE_PUBCOMP || pkt_type == MQTT_MSG_TYPE_PUBACK) {
			struct mqtt_request_t *r = mqtt_take_request(&client->pend_req_queue, pkt_id);
			if (r != NULL) {
				LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: %s response with id %d\n", mqtt_msg_type_to_str(pkt_type), pkt_id));
				if (pkt_type == MQTT_MSG_TYPE_SUBACK) {
					if (length < 3) {
						LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: To small SUBACK packet\n"));
						goto out_disconnect;
					}
					else {
						mqtt_incomming_suback(r, var_hdr_payload[2]);
					}
				}
				else if (r->cb != NULL) {
					r->cb(r->arg, ERR_OK);
				}
				mqtt_delete_request(r);
			}
			else {
				LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received %s reply, with wrong pkt_id: %d\n", mqtt_msg_type_to_str(pkt_type), pkt_id));
			}
		}
		else {
			LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received unknown message type: %d\n", pkt_type));
			goto out_disconnect;
		}
	}
	return res;
out_disconnect:
	return MQTT_CONNECT_DISCONNECTED;
}

/**
 * MQTT incoming message parser
 * @param client MQTT client
 * @param p PBUF chain of received data
 * @return Connection status
 */
static mqtt_connection_status_t
mqtt_parse_incoming(mqtt_client_t *client, struct pbuf *p)
{
	u16_t in_offset = 0;
	u32_t msg_rem_len = 0;
	u8_t fixed_hdr_len = 0;
	u8_t b = 0;

	while (p->tot_len > in_offset)
	{
		/* We ALWAYS parse the header here first. Even if the header was not
		   included in this segment, we re-parse it here by buffering it in
		   client->rx_buffer. client->msg_idx keeps track of this. */
		if ((fixed_hdr_len < 2) || ((b & 0x80) != 0))
		{

			if (fixed_hdr_len < client->msg_idx) {
				/* parse header from old pbuf (buffered in client->rx_buffer) */
				b = client->rx_buffer[fixed_hdr_len];
			}
			else {
				/* parse header from this pbuf and save it in client->rx_buffer in case
				   it comes in segmented */
				b = pbuf_get_at(p, in_offset++);
				client->rx_buffer[client->msg_idx++] = b;
			}
			fixed_hdr_len++;

			if (fixed_hdr_len >= 2) {
				/* fixed header contains at least 2 bytes but can contain more, depending on
				   'remaining length'. All bytes but the last of this have 0x80 set to
				   indicate more bytes are coming. */
				msg_rem_len |= (u32_t)(b & 0x7f) << ((fixed_hdr_len - 2) * 7);
				if ((b & 0x80) == 0) {
					/* fixed header is done */
					LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_parse_incoming: Remaining length after fixed header: %"U32_F"\n", msg_rem_len));
					if (msg_rem_len == 0) {
						/* Complete message with no extra headers of payload received */
						mqtt_message_received(client, fixed_hdr_len, 0, 0);
						client->msg_idx = 0;
						fixed_hdr_len = 0;
					}
					else {
						/* Bytes remaining in message (changes remaining length if this is
						   not the first segment of this message) */
						msg_rem_len = (msg_rem_len + fixed_hdr_len) - client->msg_idx;
					}
				}
			}
		}
		else {
			/* Fixed header has been parsed, parse variable header */
			u16_t cpy_len, buffer_space;

			/* Allow to copy the lesser one of available length in input data or bytes remaining in message */
			cpy_len = (u16_t)LWIP_MIN((u16_t)(p->tot_len - in_offset), msg_rem_len);

			/* Limit to available space in buffer */
			buffer_space = MQTT_VAR_HEADER_BUFFER_LEN - fixed_hdr_len;
			if (cpy_len > buffer_space) {
				cpy_len = buffer_space;
			}
			pbuf_copy_partial(p, client->rx_buffer + fixed_hdr_len, cpy_len, in_offset);

			/* Advance get and put indexes  */
			client->msg_idx += cpy_len;
			in_offset += cpy_len;
			msg_rem_len -= cpy_len;

			LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_parse_incoming: msg_idx: %"U32_F", cpy_len: %"U16_F", remaining %"U32_F"\n", client->msg_idx, cpy_len, msg_rem_len));
			if ((msg_rem_len == 0) || (cpy_len == buffer_space)) {
				/* Whole message received or buffer is full */
				mqtt_connection_status_t res = mqtt_message_received(client, fixed_hdr_len, cpy_len, msg_rem_len);
				if (res != MQTT_CONNECT_ACCEPTED) {
					printf("MQTT WIN32 received connect denial!\n");
					return res;
				}
				printf("MQTT WIN32 received packet!\n");
				if (msg_rem_len == 0) {
					/* Reset parser state */
					client->msg_idx = 0;
					/* msg_tot_len = 0; */
					fixed_hdr_len = 0;
				}
			}
		}
	}
	return MQTT_CONNECT_ACCEPTED;
}

void WIN_RunMQTTClient(mqtt_client_t *cl) {
	struct pbuf buf;
	byte data[8192];
	int len = 0;
	int err;

	if (cl->conn == 0) {
		return;
	}

	memset(&buf, 0, sizeof(buf));

	if (cl->conn_state == TCP_CONNECTING) {    // Check if the connection is complete
		fd_set fd;
		struct timeval time;
		FD_ZERO(&fd);
		FD_SET(cl->conn->sock, &fd);
		time.tv_sec = 0;
		time.tv_usec = 0;
		if (select(0, NULL, &fd, NULL, &time) == 1) {
			int error = 0;
			int len = sizeof(error);
			getsockopt(cl->conn->sock, SOL_SOCKET, SO_ERROR, (char*)&error, &len);
			if (error == 0) {
				printf("MQTT: Connected!\n");
				cl->conn_state = MQTT_CONNECTING;
			}
			else {
				printf("MQTT: Failed to connect: %d\n", error);
				mqtt_disconnect(cl);
			}
		}
		else {
			//printf("MQTT: Connection in progress\n");
		}
	}
	else {
		//Receive a reply from the server
		len = recv(cl->conn->sock, data, sizeof(data), 0);
		if (len > 0)
		{
			buf.payload = data;
			buf.len = len;
			buf.tot_len = len;
			mqtt_parse_incoming(cl, &buf);
		}
		else {
			err = WSAGetLastError();
			if (err == WSAEWOULDBLOCK) {

			}
			else {
				printf("MQTT socket error %i - will disconnect\n", err);
				mqtt_disconnect(cl);
				return;
			}
		}
		mqtt_output_send(&cl->output, cl->conn);
	}
}
void WIN_ResetMQTT() {
	for (int i = 0; i < g_numClients; i++) {
		mqtt_client_t *client = g_clients[i];
		if (client) {
			mqtt_disconnect(client);
			free(client);
			g_clients[i] = 0;
		}
	}
	memset(g_clients, 0, sizeof(g_clients));
	g_numClients = 0;
}
void WIN_RunMQTTFrame() {
	for (int i = 0; i < g_numClients; i++) {
		mqtt_client_t *client = g_clients[i];
		WIN_RunMQTTClient(client);
	}
}
#endif
